// $Id: ViewerGui.cpp 417 2012-08-27 13:02:40Z ahornung $

/**
 * Octomap:
 * A  probabilistic, flexible, and compact 3D mapping library for robotic systems.
 * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009-2011.
 * @see http://octomap.sourceforge.net/
 * License: GNU GPL v2, http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
 */

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include <iostream>
#include <fstream>
//#include <octomap/octomap_timing.h>

#include <octovis/ViewerGui.h>
#include <octovis/ColorOcTreeDrawer.h>
#include <octomap/MapCollection.h>


#define _MAXRANGE_URG 5.1
#define _MAXRANGE_SICK 50.0

namespace octomap{

  ViewerGui::ViewerGui(const std::string& filename, QWidget *parent)
    : QMainWindow(parent), m_scanGraph(NULL), 
      m_trajectoryDrawer(NULL), m_pointcloudDrawer(NULL), 
      m_cameraFollowMode(NULL),
      m_octreeResolution(0.1), m_laserMaxRange(-1.), m_occupancyThresh(0.5),
      m_max_tree_depth(16), m_laserType(LASERTYPE_SICK),
      m_cameraStored(false), m_filename("") {

    ui.setupUi(this);
    m_glwidget = new ViewerWidget(this);
    this->setCentralWidget(m_glwidget);

    // Settings panel at the right side
    ViewerSettingsPanel* settingsPanel = new ViewerSettingsPanel(this);
    QDockWidget* settingsDock = new QDockWidget("Octree / Scan graph settings", this);
    settingsDock->setWidget(settingsPanel);
    this->addDockWidget(Qt::RightDockWidgetArea, settingsDock);
    ui.menuShow->addAction(settingsDock->toggleViewAction());

    // Camera settings panel at the right side
    ViewerSettingsPanelCamera* settingsCameraPanel = new ViewerSettingsPanelCamera(this);
    QDockWidget *settingsCameraDock = new QDockWidget("Camera settings", this);
    settingsCameraDock->setWidget(settingsCameraPanel);
    this->addDockWidget(Qt::RightDockWidgetArea, settingsCameraDock);
    this->tabifyDockWidget(settingsDock, settingsCameraDock);
    settingsDock->raise();
    ui.menuShow->addAction(settingsCameraDock->toggleViewAction());

    // status bar
    m_mapSizeStatus = new QLabel("Map size", this);
    m_mapMemoryStatus = new QLabel("Memory consumption", this);
    m_mapSizeStatus->setFrameStyle(QFrame::Panel | QFrame::Sunken);
    m_mapMemoryStatus->setFrameStyle(QFrame::Panel | QFrame::Sunken);
    statusBar()->addPermanentWidget(m_mapSizeStatus);
    statusBar()->addPermanentWidget(m_mapMemoryStatus);

    m_cameraFollowMode = new CameraFollowMode();

    connect(this, SIGNAL(updateStatusBar(QString, int)), statusBar(), SLOT(showMessage(QString, int)));

    connect(settingsPanel, SIGNAL(treeDepthChanged(int)), this, SLOT(changeTreeDepth(int)));
    connect(settingsPanel, SIGNAL(addNextScans(unsigned)), this, SLOT(addNextScans(unsigned)));
    connect(settingsPanel, SIGNAL(gotoFirstScan()), this, SLOT(gotoFirstScan()));

    connect(settingsCameraPanel, SIGNAL(jumpToFrame(unsigned)), m_cameraFollowMode, SLOT(jumpToFrame(unsigned)));
    connect(settingsCameraPanel, SIGNAL(play()), m_cameraFollowMode, SLOT(play()));
    connect(settingsCameraPanel, SIGNAL(pause()), m_cameraFollowMode, SLOT(pause()));
    connect(settingsCameraPanel, SIGNAL(clearCameraPath()), m_cameraFollowMode, SLOT(clearCameraPath()));
    connect(settingsCameraPanel, SIGNAL(saveToCameraPath()), m_cameraFollowMode, SLOT(saveToCameraPath()));
    connect(settingsCameraPanel, SIGNAL(removeFromCameraPath()), m_cameraFollowMode, SLOT(removeFromCameraPath()));
    connect(settingsCameraPanel, SIGNAL(addToCameraPath()), m_cameraFollowMode, SLOT(addToCameraPath()));
    connect(settingsCameraPanel, SIGNAL(followCameraPath()), m_cameraFollowMode, SLOT(followCameraPath()));
    connect(settingsCameraPanel, SIGNAL(followRobotPath()), m_cameraFollowMode, SLOT(followRobotPath()));

    connect(m_cameraFollowMode, SIGNAL(changeNumberOfFrames(unsigned)), settingsCameraPanel, SLOT(setNumberOfFrames(unsigned)));
    connect(m_cameraFollowMode, SIGNAL(frameChanged(unsigned)), settingsCameraPanel, SLOT(setCurrentFrame(unsigned)));
    connect(m_cameraFollowMode, SIGNAL(stopped()), settingsCameraPanel, SLOT(setStopped()));
    connect(m_cameraFollowMode, SIGNAL(scanGraphAvailable(bool)), settingsCameraPanel, SLOT(setRobotTrajectoryAvailable(bool)));

    connect(m_cameraFollowMode, SIGNAL(deleteCameraPath(int)), m_glwidget, SLOT(deleteCameraPath(int)));
    connect(m_cameraFollowMode, SIGNAL(removeFromCameraPath(int,int)), m_glwidget, SLOT(removeFromCameraPath(int,int)));
    connect(m_cameraFollowMode, SIGNAL(appendToCameraPath(int, const octomath::Pose6D&)), m_glwidget, SLOT(appendToCameraPath(int, const octomath::Pose6D&)));
    connect(m_cameraFollowMode, SIGNAL(appendCurrentToCameraPath(int)), m_glwidget, SLOT(appendCurrentToCameraPath(int)));
    connect(m_cameraFollowMode, SIGNAL(addCurrentToCameraPath(int,int)), m_glwidget, SLOT(addCurrentToCameraPath(int,int)));
    connect(m_cameraFollowMode, SIGNAL(updateCameraPath(int,int)), m_glwidget, SLOT(updateCameraPath(int,int)));
    connect(m_cameraFollowMode, SIGNAL(playCameraPath(int,int)), m_glwidget, SLOT(playCameraPath(int,int)));
    connect(m_cameraFollowMode, SIGNAL(stopCameraPath(int)), m_glwidget, SLOT(stopCameraPath(int)));
    connect(m_cameraFollowMode, SIGNAL(jumpToCamFrame(int, int)), m_glwidget, SLOT(jumpToCamFrame(int, int)));
    connect(m_glwidget, SIGNAL(cameraPathStopped(int)), m_cameraFollowMode, SLOT(cameraPathStopped(int)));
    connect(m_glwidget, SIGNAL(cameraPathFrameChanged(int, int)), m_cameraFollowMode, SLOT(cameraPathFrameChanged(int, int)));

    connect(this, SIGNAL(changeNumberOfScans(unsigned)), settingsPanel, SLOT(setNumberOfScans(unsigned)));
    connect(this, SIGNAL(changeCurrentScan(unsigned)), settingsPanel, SLOT(setCurrentScan(unsigned)));
    connect(this, SIGNAL(changeResolution(double)), settingsPanel, SLOT(setResolution(double)));

    connect(settingsCameraPanel, SIGNAL(changeCamPosition(double, double, double, double, double, double)), 
            m_glwidget, SLOT(setCamPosition(double, double, double, double, double, double)));
    connect(m_cameraFollowMode, SIGNAL(changeCamPose(const octomath::Pose6D&)),
            m_glwidget, SLOT(setCamPose(const octomath::Pose6D&)));

    connect(ui.actionReset_view, SIGNAL(triggered()), m_glwidget, SLOT(resetView()));

    if (filename != ""){
      m_filename = filename;
      openFile();
    }
  }

  ViewerGui::~ViewerGui() {
    if (m_trajectoryDrawer){
      m_glwidget->removeSceneObject(m_trajectoryDrawer);
      delete m_trajectoryDrawer;
      m_trajectoryDrawer = NULL;
    }

    if (m_pointcloudDrawer){
      m_glwidget->removeSceneObject(m_pointcloudDrawer);
      delete m_pointcloudDrawer;
      m_pointcloudDrawer = NULL;
    }

    for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); it != m_octrees.end(); ++it) {
      m_glwidget->removeSceneObject(it->second.octree_drawer);
      delete (it->second.octree_drawer);
      delete (it->second.octree);
    }
    m_octrees.clear();

    if(m_cameraFollowMode) {
      delete m_cameraFollowMode;
      m_cameraFollowMode = NULL;
    }
  }

  bool ViewerGui::isShown() {
    return  m_glwidget->isVisible();
  }

  void ViewerGui::showInfo(QString string, bool newline) {
    std::cerr << string.toLocal8Bit().data();
    if (newline) std::cerr << std::endl;
    else std::cerr << std::flush;
    int duration = 0;
    if (newline)
      duration = 3000;
    emit updateStatusBar(string, duration);
  }

  bool ViewerGui::getOctreeRecord(int id, OcTreeRecord*& otr) {
    std::map<int, OcTreeRecord>::iterator it = m_octrees.find(id);
    if( it != m_octrees.end() ) {
      otr = &(it->second);
      return true;
    }
    else {
      return false;
    } 
  }

  void ViewerGui::addOctree(octomap::AbstractOcTree* tree, int id, octomap::pose6d origin) {
    // is id in use?
    OcTreeRecord* r;
    bool foundRecord = getOctreeRecord(id, r);
    if (foundRecord && r->octree->getTreeType().compare(tree->getTreeType()) !=0){
      // delete old drawer, create new
      delete r->octree_drawer;
      if (dynamic_cast<OcTree*>(tree)) {
        r->octree_drawer = new OcTreeDrawer();
        //        fprintf(stderr, "adding new OcTreeDrawer for node %d\n", id);
      }
      else if (dynamic_cast<ColorOcTree*>(tree)) {
        r->octree_drawer = new ColorOcTreeDrawer();
      } else{
        OCTOMAP_ERROR("Could not create drawer for tree type %s\n", tree->getTreeType().c_str());
      }

      delete r->octree;
      r->octree = tree;
      r->origin = origin;

    } else if (foundRecord && r->octree->getTreeType().compare(tree->getTreeType()) ==0) {
      // only swap out tree

      delete r->octree;
      r->octree = tree;
      r->origin = origin;
    } else {
      // add new record
      OcTreeRecord otr;
      otr.id = id;
      if (dynamic_cast<OcTree*>(tree)) {      
        otr.octree_drawer = new OcTreeDrawer();
        //        fprintf(stderr, "adding new OcTreeDrawer for node %d\n", id);
      }
      else if (dynamic_cast<ColorOcTree*>(tree)) {      
        otr.octree_drawer = new ColorOcTreeDrawer();
      } else{
        OCTOMAP_ERROR("Could not create drawer for tree type %s\n", tree->getTreeType().c_str());
      }
      otr.octree = tree;
      otr.origin = origin;
      m_octrees[id] = otr;
      m_glwidget->addSceneObject(otr.octree_drawer);
    }
  }

  void ViewerGui::addOctree(octomap::AbstractOcTree* tree, int id) {
    octomap::pose6d o; // initialized to (0,0,0) , (0,0,0,1) by default
    addOctree(tree, id, o);
  }

  void ViewerGui::showOcTree() {

    // update viewer stat
      double minX, minY, minZ, maxX, maxY, maxZ;
      minX = minY = minZ = -10; // min bbx for drawing
      maxX = maxY = maxZ = 10;  // max bbx for drawing
      double sizeX, sizeY, sizeZ;
      sizeX = sizeY = sizeZ = 0.;
      size_t memoryUsage = 0;
      size_t num_nodes = 0;
      size_t memorySingleNode = 0;

    
      for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); it != m_octrees.end(); ++it) {
        // get map bbx
        double lminX, lminY, lminZ, lmaxX, lmaxY, lmaxZ;
        it->second.octree->getMetricMin(lminX, lminY, lminZ);
        it->second.octree->getMetricMax(lmaxX, lmaxY, lmaxZ);
        // transform to world coords using map origin
        octomap::point3d pmin(lminX, lminY, lminZ);
        octomap::point3d pmax(lmaxX, lmaxY, lmaxZ);
        pmin = it->second.origin.transform(pmin);
        pmax = it->second.origin.transform(pmax);
        lminX = pmin.x(); lminY = pmin.y(); lminZ = pmin.z();
        lmaxX = pmax.x(); lmaxY = pmax.y(); lmaxZ = pmax.z();
        // update global bbx
        if (lminX < minX) minX = lminX;
        if (lminY < minY) minY = lminY;
        if (lminZ < minZ) minZ = lminZ;
        if (lmaxX > maxX) maxX = lmaxX;
        if (lmaxY > maxY) maxY = lmaxY;
        if (lmaxZ > maxZ) maxZ = lmaxZ;
        double lsizeX, lsizeY, lsizeZ;
        // update map stats
        it->second.octree->getMetricSize(lsizeX, lsizeY, lsizeZ);
        if (lsizeX > sizeX) sizeX = lsizeX;
        if (lsizeY > sizeY) sizeY = lsizeY;
        if (lsizeZ > sizeZ) sizeZ = lsizeZ;
        memoryUsage += it->second.octree->memoryUsage();
        num_nodes += it->second.octree->size();
        memorySingleNode = std::max(memorySingleNode, it->second.octree->memoryUsageNode());
      }

      m_glwidget->setSceneBoundingBox(qglviewer::Vec(minX, minY, minZ), qglviewer::Vec(maxX, maxY, maxZ));

    //if (m_octrees.size()) {
      QString size = QString("%L1 x %L2 x %L3 m^3; %L4 nodes").arg(sizeX).arg(sizeY).arg(sizeZ).arg(unsigned(num_nodes));
      QString memory = QString("Single node: %L1 B; ").arg(memorySingleNode)
        + QString ("Octree: %L1 B (%L2 MB)").arg(memoryUsage).arg((double) memoryUsage/(1024.*1024.), 0, 'f', 3);
      m_mapMemoryStatus->setText(memory);
      m_mapSizeStatus->setText(size);
    //}

    m_glwidget->updateGL();

    // generate cubes -> display
   // timeval start; 
   // timeval stop; 
   // gettimeofday(&start, NULL);  // start timer    
    for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); it != m_octrees.end(); ++it) {
      it->second.octree_drawer->setMax_tree_depth(m_max_tree_depth);
      it->second.octree_drawer->setOcTree(*it->second.octree, it->second.origin, it->second.id);
    }
//    gettimeofday(&stop, NULL);  // stop timer
//    double time_to_generate = (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec);
//    fprintf(stderr, "setOcTree took %f sec\n", time_to_generate);
    m_glwidget->updateGL();
  }


  void ViewerGui::generateOctree() {

    if (m_scanGraph) {

      QApplication::setOverrideCursor(Qt::WaitCursor);

      showInfo("Generating OcTree... ");
      std::cerr << std::endl;

      //if (m_ocTree) delete m_ocTree;
      OcTree* tree = new octomap::OcTree(m_octreeResolution);

      octomap::ScanGraph::iterator it;
      unsigned numScans = m_scanGraph->size();
      unsigned currentScan = 1;
      for (it = m_scanGraph->begin(); it != m_nextScanToAdd; it++) {
        tree->insertScan(**it, m_laserMaxRange);
        fprintf(stderr, "generateOctree:: inserting scan node with %d points, origin: %.2f  ,%.2f , %.2f.\n",
                (unsigned int) (*it)->scan->size(), (*it)->pose.x(), (*it)->pose.y(), (*it)->pose.z()  );

        std::cout << " S ("<<currentScan<<"/"<<numScans<<") " << std::flush;
        currentScan++;
      }

      this->addOctree(tree, DEFAULT_OCTREE_ID);
      this->showOcTree();

      showInfo("Done.", true);
      QApplication::restoreOverrideCursor();
    }
    else {
      std::cerr << "generateOctree called but no ScanGraph present!\n";
    }

  }


  // void ViewerGui::handleOctomapBinaryMsg(const octomap2::OctomapBinary::ConstPtr& msg) {

  //   printf("received octomap, tree id=%d, origin: <%.2f , %.2f , %.2f>, <%.2f , %.2f , %.2f , %.2f>\n", 
  //          msg->id, msg->origin.position.x, msg->origin.position.y, msg->origin.position.z,
  //          msg->origin.orientation.x, msg->origin.orientation.y, msg->origin.orientation.z,
  //          msg->origin.orientation.w
  //          );

  //   OcTree* tree = new octomap::OcTree(0.05);
  //   octomap::octomapMsgToMap(*msg, *tree);

  //   octomath::Vector3 trans (msg->origin.position.x, msg->origin.position.y, msg->origin.position.z);
  //   octomath::Quaternion rot (msg->origin.orientation.w, msg->origin.orientation.x, msg->origin.orientation.y, msg->origin.orientation.z);
  //   std::cout << rot << std::endl;
  //   octomap::pose6d origin (trans, rot);

  //   this->addOctree(tree, msg->id, origin);

  //   m_octreeResolution = tree->getResolution();
  //   emit changeResolution(m_octreeResolution);

  //   ui.actionPointcloud->setChecked(false);
  //   ui.actionPointcloud->setEnabled(false);
  //   ui.actionOctree_cells->setChecked(true);
  //   ui.actionOctree_cells->setEnabled(true);
  //   ui.actionFree->setChecked(false);
  //   ui.actionFree->setEnabled(true);
  //   ui.actionOctree_structure->setEnabled(true);
  //   ui.actionOctree_structure->setChecked(false);
  //   ui.actionTrajectory->setEnabled(false);
  //   ui.actionConvert_ml_tree->setEnabled(false);
  //   ui.actionReload_Octree->setEnabled(false);
  //   ui.actionSettings->setEnabled(false);

  //   showOcTree();
  //   //    m_glwidget->resetView();
  // }


  // void ViewerGui::handleMoveMapMsg(const octomap2::MoveMap::ConstPtr& msg) {

  //   octomath::Vector3 trans (msg->transform.position.x, msg->transform.position.y, msg->transform.position.z);
  //   octomath::Quaternion rot (msg->transform.orientation.w, msg->transform.orientation.x, msg->transform.orientation.y, msg->transform.orientation.z);
  //   octomap::pose6d transform (trans, rot);
  //   std::cout << transform << std::endl;
    
  //   OcTreeRecord* r;
  //   if (getOctreeRecord(msg->id, r)) {

  //     if (msg->absolute) {  // absolute movement -> set origin

  //       r->origin = transform;
  //       r->octree_drawer->setOrigin(transform);

  //       fprintf(stderr, "received new origin for map id=%d: <%.2f , %.2f , %.2f>, <%.2f , %.2f , %.2f , %.2f>\n", 
  //               msg->id, msg->transform.position.x, msg->transform.position.y, msg->transform.position.z,
  //               msg->transform.orientation.x, msg->transform.orientation.y, msg->transform.orientation.z,
  //               msg->transform.orientation.w
  //               );
  //     }

  //     else { // relative movement -> transform pose

  //       octomap::pose6d T = r->origin;
  //       octomath::Quaternion rot_new = T.rot() * transform.rot();
  //       octomap::point3d trans_new = T.trans() + transform.trans();

  //       r->origin = octomap::pose6d(trans_new, rot_new);
  //       r->octree_drawer->setOrigin(r->origin);

  //       fprintf(stderr, "received relative transform for map id=%d: <%.2f , %.2f , %.2f>, <%.2f , %.2f , %.2f , %.2f>\n", 
  //               msg->id, msg->transform.position.x, msg->transform.position.y, msg->transform.position.z,
  //               msg->transform.orientation.x, msg->transform.orientation.y, msg->transform.orientation.z,
  //               msg->transform.orientation.w
  //               );
  //     }
  //     m_glwidget->updateGL();
  //   }
  //   else {
  //     fprintf(stderr, "ERROR: map %d does not exist\n", msg->id);
  //   }
  // }



  // ==  incremental graph generation   =======================

  void ViewerGui::gotoFirstScan(){
    if (m_scanGraph){
      showInfo("Inserting first scan node into tree... ", true);
      QApplication::setOverrideCursor(Qt::WaitCursor);

      m_nextScanToAdd = m_scanGraph->begin();
    
      // if (m_ocTree) delete m_ocTree;
      // m_ocTree = new octomap::OcTree(m_octreeResolution);
      OcTree* tree = new octomap::OcTree(m_octreeResolution);
      this->addOctree(tree, DEFAULT_OCTREE_ID);

      addNextScan();

      QApplication::restoreOverrideCursor();
      showOcTree();
    }
  }

  void ViewerGui::addNextScan(){

    if (m_scanGraph){
      showInfo("Inserting next scan node into tree... ", true);

      QApplication::setOverrideCursor(Qt::WaitCursor);
      if (m_nextScanToAdd != m_scanGraph->end()){
        OcTreeRecord* r;
        if (!getOctreeRecord(DEFAULT_OCTREE_ID, r)) {
          fprintf(stderr, "ERROR: OctreeRecord for id %d not found!\n", DEFAULT_OCTREE_ID);
          return;
        }
        // not used with ColorOcTrees, omitting casts
        ((OcTree*) r->octree)->insertScan(**m_nextScanToAdd, m_laserMaxRange); 
        m_nextScanToAdd++;
      }

      QApplication::restoreOverrideCursor();
      showOcTree();

    }
  }


  void ViewerGui::addNextScans(unsigned scans){
    for (unsigned i = 0; i < scans; ++i){
      addNextScan();
    }
  }



  // ==  file I/O   ===========================================

  void ViewerGui::openFile(){
    if (!m_filename.empty()){
      m_glwidget->clearAll();

      QString temp = QString(m_filename.c_str());
      QFileInfo fileinfo(temp);
      if (fileinfo.suffix() == "graph"){
        openGraph();
      }else if (fileinfo.suffix() == "bt"){
        openTree();
      }
      else if (fileinfo.suffix() == "ot")
      {
        openOcTree();
      }
      else if (fileinfo.suffix() == "hot"){
        openMapCollection();
      }
      else if (fileinfo.suffix() == "dat"){
        openPointcloud();
      }
      else {
        QMessageBox::warning(this, "Unknown file", "Cannot open file, unknown extension: "+fileinfo.suffix(), QMessageBox::Ok);
      }
    }
  }


  void ViewerGui::openGraph(bool completeGraph){

    QApplication::setOverrideCursor(Qt::WaitCursor);
    showInfo("Loading scan graph from file " + QString(m_filename.c_str()) );  

    if (m_scanGraph) delete m_scanGraph;
    m_scanGraph = new octomap::ScanGraph();
    m_scanGraph->readBinary(m_filename);

    loadGraph(completeGraph);
  }


  void ViewerGui::openPointcloud(){
  
    QApplication::setOverrideCursor(Qt::WaitCursor);
    showInfo("Loading ASCII pointcloud from file "+QString(m_filename.c_str()) + "..."); 

    if (m_scanGraph) delete m_scanGraph;
    m_scanGraph = new octomap::ScanGraph();


    // read pointcloud from file
    std::ifstream s(m_filename.c_str());
    Pointcloud* pc = new Pointcloud();

    if (!s) {
      std::cout <<"ERROR: could not read " << m_filename << std::endl;
      return;
    }

    pc->read(s);

    pose6d laser_pose(0,0,0,0,0,0);
    m_scanGraph->addNode(pc, laser_pose);

    loadGraph(true);
    showInfo("Done.", true);
  }


  void ViewerGui::setOcTreeUISwitches() {
    ui.actionPointcloud->setChecked(false);
    ui.actionPointcloud->setEnabled(false);
    ui.actionOctree_cells->setChecked(true);
    ui.actionOctree_cells->setEnabled(true);
    ui.actionFree->setChecked(false);
    ui.actionFree->setEnabled(true);
    ui.actionOctree_structure->setEnabled(true);
    ui.actionOctree_structure->setChecked(false);
    ui.actionTrajectory->setEnabled(false);
    ui.actionConvert_ml_tree->setEnabled(true);
    ui.actionReload_Octree->setEnabled(true);
    ui.actionSettings->setEnabled(false);
  }

  void ViewerGui::openTree(){
    OcTree* tree = new octomap::OcTree(m_filename);
    this->addOctree(tree, DEFAULT_OCTREE_ID);

    m_octreeResolution = tree->getResolution();
    emit changeResolution(m_octreeResolution);

    setOcTreeUISwitches();
    showOcTree();
    m_glwidget->resetView();
  }

  void ViewerGui::openOcTree(){
    AbstractOcTree* tree = AbstractOcTree::read(m_filename);

    if (tree){
      this->addOctree(tree, DEFAULT_OCTREE_ID);

      m_octreeResolution = tree->getResolution();
      emit changeResolution(m_octreeResolution);

      setOcTreeUISwitches();
      showOcTree();
      m_glwidget->resetView();

      if (tree->getTreeType() == "ColorOcTree"){
        // map color and height map share the same color array and QAction
        ui.actionHeight_map->setText ("Map color");  // rename QAction in Menu
        this->on_actionHeight_map_toggled(true); // enable color view
        ui.actionHeight_map->setChecked(true);
      }
    } 
    else {
      QMessageBox::warning(this, "File error", "Cannot open OcTree file", QMessageBox::Ok);
    }
  }


  // EXPERIMENTAL
  void ViewerGui::openMapCollection() {

    OCTOMAP_DEBUG("Opening hierarchy from %s...\n", m_filename.c_str());

    std::ifstream infile(m_filename.c_str(), std::ios_base::in |std::ios_base::binary);
    if (!infile.is_open()) {
      QMessageBox::warning(this, "File error", "Cannot open OcTree file", QMessageBox::Ok);
      return;
    }
    infile.close();
    
    MapCollection<MapNode<OcTree> > collection(m_filename);
    int i=0;
    for (MapCollection<MapNode<OcTree> >::iterator it = collection.begin(); 
         it != collection.end(); ++it) {
      OCTOMAP_DEBUG("Adding hierarchy node %s\n", (*it)->getId().c_str());
      OcTree* tree = (*it)->getMap();
      if (!tree)
        OCTOMAP_ERROR("Error while reading node %s\n", (*it)->getId().c_str());
      else {
        OCTOMAP_DEBUG("Read tree with %zu tree nodes\n", tree->size());
      }
      pose6d  origin = (*it)->getOrigin();
      this->addOctree(tree, i, origin);
      ++i;
    }
    setOcTreeUISwitches();
    showOcTree();
    m_glwidget->resetView();
    OCTOMAP_DEBUG("done\n");
  }

  void ViewerGui::loadGraph(bool completeGraph) {
  
    ui.actionSettings->setEnabled(true);
    ui.actionPointcloud->setEnabled(true);
    ui.actionPointcloud->setChecked(false);
    ui.actionTrajectory->setEnabled(true);
    ui.actionOctree_cells->setEnabled(true);
    ui.actionOctree_cells->setChecked(true);
    ui.actionOctree_structure->setEnabled(true);
    ui.actionOctree_structure->setChecked(false);
    ui.actionFree->setChecked(false);
    ui.actionFree->setEnabled(true);
    ui.actionReload_Octree->setEnabled(true);
    ui.actionConvert_ml_tree->setEnabled(true);

    unsigned graphSize = m_scanGraph->size();
    unsigned currentScan;
  
    if (completeGraph){
      fprintf(stderr, "loadGraph:: generating octree from complete graph.\n" );
      m_nextScanToAdd = m_scanGraph->end();
      generateOctree();
      currentScan = graphSize;
    } 
    else{
      m_nextScanToAdd = m_scanGraph->begin();
    
      //if (m_ocTree) delete m_ocTree;
      //m_ocTree = new octomap::OcTree(m_octreeResolution);
      OcTree* tree = new octomap::OcTree(m_octreeResolution);
      this->addOctree(tree, DEFAULT_OCTREE_ID);

      addNextScan();

      currentScan = 1;
    }

    m_glwidget->resetView();
    QApplication::restoreOverrideCursor();

    emit changeNumberOfScans(graphSize);
    emit changeCurrentScan(currentScan);
    showInfo("Done (" +QString::number(currentScan)+ " of "+ QString::number(graphSize)+" nodes)", true);

    if (!m_trajectoryDrawer){
      m_trajectoryDrawer = new TrajectoryDrawer();
    }
    m_trajectoryDrawer->setScanGraph(*m_scanGraph);

    if (!m_pointcloudDrawer){
      m_pointcloudDrawer = new PointcloudDrawer();
    }
    m_pointcloudDrawer->setScanGraph(*m_scanGraph);

    m_cameraFollowMode->setScanGraph(m_scanGraph);

    if (ui.actionTrajectory->isChecked())
      m_glwidget->addSceneObject(m_trajectoryDrawer);

    if (ui.actionPointcloud->isChecked())
      m_glwidget->addSceneObject(m_pointcloudDrawer);
  }

  void ViewerGui::changeTreeDepth(int depth){
    // range check:
    if (depth < 1 || depth > 16)
      return;

    m_max_tree_depth = unsigned(depth);

    if (m_octrees.size() > 0)
      showOcTree();
  }


  void ViewerGui::on_actionExit_triggered(){
    this->close();
  }

  void ViewerGui::on_actionHelp_triggered(){
    m_glwidget->help();
  }

  void ViewerGui::on_actionSettings_triggered(){

    ViewerSettings dialog(this);
    dialog.setResolution(m_octreeResolution);
    dialog.setLaserType(m_laserType);
    dialog.setMaxRange(m_laserMaxRange);


    if (dialog.exec()){

      double oldResolution = m_octreeResolution;
      double oldLaserMaxRange = m_laserMaxRange;
      double oldType = m_laserType;

      m_octreeResolution = dialog.getResolution();
      m_laserType = dialog.getLaserType();
      m_laserMaxRange = dialog.getMaxRange();
  
      // apply new settings
      bool resolutionChanged = (std::abs(oldResolution - m_octreeResolution) > 1e-5);
      bool maxRangeChanged = (std::abs(oldLaserMaxRange - m_laserMaxRange) > 1e-5);

      if (resolutionChanged)
        emit changeResolution(m_octreeResolution);

      if (oldType != m_laserType){ // parameters changed, reload file:
        openFile();
      } else if (resolutionChanged || maxRangeChanged){
        generateOctree();
      }

    }
  }

  void ViewerGui::on_actionOpen_file_triggered(){
    QString filename = QFileDialog::getOpenFileName(this,
      tr("Open data file"), "",
      "All supported files (*.graph *.bt *.ot *.dat);;OcTree file (*.ot);;Bonsai tree file (*.bt);;Binary scan graph (*.graph);;Pointcloud (*.dat);;All files (*)");
    if (!filename.isEmpty()){
#ifdef _WIN32      
      m_filename = std::string(filename.toLocal8Bit().data());
#else       
      m_filename = filename.toStdString();
#endif
      openFile();
    }
  }


  void ViewerGui::on_actionOpen_graph_incremental_triggered(){
    QString filename = QFileDialog::getOpenFileName(this,
     tr("Open graph file incrementally (at start)"), "",
     "Binary scan graph (*.graph)");
    if (!filename.isEmpty()){
      m_glwidget->clearAll();

#ifdef _WIN32      
      m_filename = std::string(filename.toLocal8Bit().data());
#else       
      m_filename = filename.toStdString();
#endif

      openGraph(false);
    }
  }

  void ViewerGui::on_actionSave_file_triggered(){

    OcTreeRecord* r;
    if (!getOctreeRecord(DEFAULT_OCTREE_ID, r)) {
      fprintf(stderr, "ERROR: OctreeRecord for id %d not found!\n", DEFAULT_OCTREE_ID);
      QMessageBox::warning(this, tr("3D Mapping Viewer"),
                           "Error: No OcTree present.",
                           QMessageBox::Ok);
      return;
    }

    QString filename = QFileDialog::getSaveFileName(this, tr("Save octree file"),
                                                    "", tr("Full OcTree (*.ot);;Bonsai Tree file (*.bt);;"));

    if (filename != ""){
      QApplication::setOverrideCursor(Qt::WaitCursor);
      showInfo("Writing file... ", false);
    
      QFileInfo fileinfo(filename);
      std::string std_filename;
#ifdef _WIN32      
      std_filename = filename.toLocal8Bit().data();
#else       
      std_filename = filename.toStdString();
#endif

      AbstractOcTree* t = r->octree;

      // TODO: get rid of casts (requires occupancy tree interface)
      if (fileinfo.suffix() == "bt") {
        AbstractOccupancyOcTree* ot = dynamic_cast<AbstractOccupancyOcTree*> (t);
        if (ot)
          ot->writeBinaryConst(std_filename);
        else{
          QMessageBox::warning(this, "Unknown tree type",
                                       "Could not convert to occupancy tree for writing .bt file",
                                       QMessageBox::Ok);
        }
      }
      else if (fileinfo.suffix() == "ot"){
        r->octree->write(std_filename);
      }
      else {
        QMessageBox::warning(this, "Unknown file", 
                             "Cannot write file, unknown extension: "+fileinfo.suffix(),
                             QMessageBox::Ok);
      }
    
      QApplication::restoreOverrideCursor();
      showInfo("Done.", true);
    }
  }

  void ViewerGui::on_actionClear_nodes_in_selection_triggered(){
	  point3d min, max;
	  m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
	  m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());

	  updateNodesInBBX(min, max, false);
  }

  void ViewerGui::on_actionClear_selection_triggered(){
	  point3d min, max;
	  m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
	  m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());

	  setNodesInBBX(min, max, false);
  }

  void ViewerGui::on_actionFill_selection_triggered(){
	  point3d min, max;
	  m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
	  m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());

	  setNodesInBBX(min, max, true);
  }

  void ViewerGui::on_actionFill_nodes_in_selection_triggered(){
	  point3d min, max;
	  m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
	  m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());

	  updateNodesInBBX(min, max, true);
  }

  void ViewerGui::on_actionDelete_nodes_in_selection_triggered(){
    point3d min, max;
    m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
    m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());

    for (std::map<int, OcTreeRecord>::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) {
      OcTree* octree = dynamic_cast<OcTree*>(t_it->second.octree);

      if (octree){
        for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max),
            end=octree->end_leafs_bbx(); it!= end; ++it){
          octree->deleteNode(it.getKey(), it.getDepth());
        }
      } else{
        QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type",
                                     QMessageBox::Ok);

      }
    }

    showOcTree();
  }

  void ViewerGui::on_actionDelete_nodes_outside_of_selection_triggered(){
    point3d min, max;
    m_glwidget->selectionBox().getBBXMin(min.x(), min.y(), min.z());
    m_glwidget->selectionBox().getBBXMax(max.x(), max.y(), max.z());

    for (std::map<int, OcTreeRecord>::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) {
      OcTree* octree = dynamic_cast<OcTree*>(t_it->second.octree);

      if (octree){
        octomap::OcTreeKey minKey, maxKey;

        if (!octree->coordToKeyChecked(min, minKey) || !octree->coordToKeyChecked(max, maxKey)){
          return;
        }

        for(OcTree::leaf_iterator it = octree->begin_leafs(),
            end=octree->end_leafs(); it!= end; ++it){
          // check if outside of bbx:
          OcTreeKey k = it.getKey();
          if  (k[0] < minKey[0] || k[1] < minKey[1] || k[2] < minKey[2]
            || k[0] > maxKey[0] || k[1] > maxKey[1] || k[2] > maxKey[2])
          {
            octree->deleteNode(k, it.getDepth());
          }
        }
      } else
        QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type",
                                     QMessageBox::Ok);
    }

    showOcTree();
  }



  void ViewerGui::updateNodesInBBX(const point3d& min, const point3d& max, float logodds){
	  for (std::map<int, OcTreeRecord>::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) {
	    OcTree* octree = dynamic_cast<OcTree*>(t_it->second.octree);

	    if (octree){

	    	for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max),
	    			end=octree->end_leafs_bbx(); it!= end; ++it){
	    		octree->updateNode(it.getKey(), logodds);
	    	}
	    } else
        QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type",
                                     QMessageBox::Ok);

	  }

	  showOcTree();
  }


  void ViewerGui::updateNodesInBBX(const point3d& min, const point3d& max, bool occupied){
	  for (std::map<int, OcTreeRecord>::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) {
	    OcTree* octree = dynamic_cast<OcTree*>(t_it->second.octree);

	    if (octree){
	      float logodds = octree->getClampingThresMaxLog() - octree->getClampingThresMinLog();
	      if (!occupied)
	        logodds *= -1;

	    	for(OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max),
	    			end=octree->end_leafs_bbx(); it!= end; ++it){
	    		octree->updateNode(it.getKey(), logodds);
	    	}
	    } else
        QMessageBox::warning(this, "Not implemented", "Functionality not yet implemented for this octree type",
                                     QMessageBox::Ok);

	  }

	  showOcTree();
  }


  void ViewerGui::setNodesInBBX(const point3d& min, const point3d& max, float logodds){
    for (std::map<int, OcTreeRecord>::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) {
      OcTree* octree = dynamic_cast<OcTree*>(t_it->second.octree);

      if (octree){
        OcTreeKey minKey(0,0,0);
        OcTreeKey maxKey(0,0,0);
        octree->coordToKeyChecked(min, minKey);
        octree->coordToKeyChecked(max, maxKey);
        OcTreeKey k;
        for (k[0] = minKey[0]; k[0] < maxKey[0]; ++k[0]){
          for (k[1] = minKey[1]; k[1] < maxKey[1]; ++k[1]){
            for (k[2] = minKey[2]; k[2] < maxKey[2]; ++k[2]){
              octree->updateNode(k, logodds);
            }
          }
        }
      }

    }

    showOcTree();
  }

  void ViewerGui::setNodesInBBX(const point3d& min, const point3d& max, bool occupied){
    for (std::map<int, OcTreeRecord>::iterator t_it = m_octrees.begin(); t_it != m_octrees.end(); ++t_it) {
      OcTree* octree = dynamic_cast<OcTree*>(t_it->second.octree);

      if (octree){
        float logodds = octree->getClampingThresMaxLog() - octree->getClampingThresMinLog();
        if (!occupied)
          logodds *= -1;

        OcTreeKey minKey(0,0,0);
        OcTreeKey maxKey(0,0,0);
        octree->coordToKeyChecked(min, minKey);
        octree->coordToKeyChecked(max, maxKey);
        OcTreeKey k;
        for (k[0] = minKey[0]; k[0] < maxKey[0]; ++k[0]){
          for (k[1] = minKey[1]; k[1] < maxKey[1]; ++k[1]){
            for (k[2] = minKey[2]; k[2] < maxKey[2]; ++k[2]){
              octree->updateNode(k, logodds);
            }
          }
        }
      }

    }

    showOcTree();
  }

  void ViewerGui::on_actionExport_view_triggered(){
    m_glwidget->openSnapshotFormatDialog();
    m_glwidget->saveSnapshot(false);
  }

  void ViewerGui::on_actionExport_sequence_triggered(bool checked){
    if(checked) {
      if(m_glwidget->openSnapshotFormatDialog()) {
        m_glwidget->saveSnapshot(false);
        m_glwidget->setSnapshotCounter(0);
        connect(m_glwidget, SIGNAL(drawFinished(bool)), m_glwidget, SLOT(saveSnapshot(bool)));
      } else {
        ui.actionExport_sequence->setChecked(false);
      }
    } else {
      disconnect(m_glwidget, SIGNAL(drawFinished(bool)), m_glwidget, SLOT(saveSnapshot(bool)));
    }
  }

  void ViewerGui::on_actionPrintout_mode_toggled(bool checked){
    if (checked) {
      ui.actionHeight_map->setChecked(false);
      ui.actionSemanticColoring->setChecked(false);
    }

    m_glwidget->enablePrintoutMode(checked);
  }

  void ViewerGui::on_actionSelection_box_toggled(bool checked){
	  ui.actionClear_selection->setEnabled(checked);
	  ui.actionFill_selection->setEnabled(checked);
	  ui.actionClear_nodes_in_selection->setEnabled(checked);
	  ui.actionFill_nodes_in_selection->setEnabled(checked);
	  ui.actionDelete_nodes_in_selection->setEnabled(checked);
	  ui.actionDelete_nodes_outside_of_selection->setEnabled(checked);

    m_glwidget->enableSelectionBox(checked);


	  m_glwidget->updateGL();
  }

  void ViewerGui::on_actionHeight_map_toggled(bool checked){
    if (checked) {
      ui.actionPrintout_mode->setChecked(false);
      ui.actionSemanticColoring->setChecked(false);
    }
    m_glwidget->enableHeightColorMode(checked);
  }

  void ViewerGui::on_actionSemanticColoring_toggled(bool checked) {
    if (checked) {
      ui.actionHeight_map->setChecked(false);
      ui.actionPrintout_mode->setChecked(false);
    }

    m_glwidget->enableSemanticColoring(checked);
  }


  void ViewerGui::on_actionStore_camera_triggered(){
    m_glwidget->camera()->deletePath(0);
    m_glwidget->camera()->addKeyFrameToPath(0);
    m_cameraStored = true;
    ui.actionRestore_camera->setEnabled(true);
  }

  void ViewerGui::on_actionRestore_camera_triggered(){
    if (m_cameraStored){
      m_glwidget->camera()->playPath(0);
    }
  }

  void ViewerGui::on_actionPointcloud_toggled(bool checked){
    if (m_pointcloudDrawer){
      if (checked)
        m_glwidget->addSceneObject(m_pointcloudDrawer);
      else
        m_glwidget->removeSceneObject(m_pointcloudDrawer);
    }
  }

  void ViewerGui::on_actionTrajectory_toggled(bool checked){
    if (m_trajectoryDrawer){
      if (checked)
        m_glwidget->addSceneObject(m_trajectoryDrawer);
      else
        m_glwidget->removeSceneObject(m_trajectoryDrawer);
    }
  }

  void ViewerGui::on_actionAxes_toggled(bool checked){
    for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); 
         it != m_octrees.end(); ++it) {
      it->second.octree_drawer->enableAxes(checked);
    }
    m_glwidget->updateGL();
  }

  void ViewerGui::on_actionHideBackground_toggled(bool checked) {
    OcTreeRecord* r;
    if (getOctreeRecord(DEFAULT_OCTREE_ID, r)) {
      if (checked) m_glwidget->removeSceneObject(r->octree_drawer);
      else         m_glwidget->addSceneObject(r->octree_drawer);
      m_glwidget->updateGL();
    }
  }

  void ViewerGui::on_actionClear_triggered() {
    for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); 
         it != m_octrees.end(); ++it) {
      m_glwidget->removeSceneObject(it->second.octree_drawer);
      delete (it->second.octree_drawer);
      delete (it->second.octree);
    }
    m_octrees.clear();
    showOcTree();
  }

  void ViewerGui::on_actionTest_triggered(){

  }

  void ViewerGui::on_actionReload_Octree_triggered(){
    if (m_scanGraph) {
      generateOctree();
    } else {
      openFile();
    }
  }

  void ViewerGui::on_actionConvert_ml_tree_triggered(){
    QApplication::setOverrideCursor(Qt::WaitCursor);
    if (m_octrees.size()) {
      showInfo("Converting OcTree to maximum Likelihood map... ");
      for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); 
           it != m_octrees.end(); ++it) {
        AbstractOcTree* t = it->second.octree;
        if (dynamic_cast<OcTree*>(t)) {
          ((OcTree*) t)->toMaxLikelihood();
        }
        else if (dynamic_cast<OcTree*>(t)) {
          ((ColorOcTree*) t)->toMaxLikelihood();
        }
      }
      showInfo("Done.", true);
      showOcTree();
      QApplication::restoreOverrideCursor();
    }
  }


  void ViewerGui::on_actionPrune_tree_triggered(){
    QApplication::setOverrideCursor(Qt::WaitCursor);
    if (m_octrees.size()) { 
      showInfo("Pruning OcTree... ");
      for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); 
           it != m_octrees.end(); ++it) {
        it->second.octree->prune();
      }
      showOcTree();
      showInfo("Done.", true);
    }
    QApplication::restoreOverrideCursor();
  }


  void ViewerGui::on_actionExpand_tree_triggered(){

    QApplication::setOverrideCursor(Qt::WaitCursor);

    // if (m_ocTree) {
    if (m_octrees.size()) {
      showInfo("Expanding OcTree... ");
      for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); 
           it != m_octrees.end(); ++it) {
        it->second.octree->expand();
      }
      showOcTree();
      
      showInfo("Done.", true);
    }    
    QApplication::restoreOverrideCursor();
  }


  void ViewerGui::on_actionOctree_cells_toggled(bool enabled) {
    for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); 
         it != m_octrees.end(); ++it) {
      it->second.octree_drawer->enableOcTreeCells(enabled);
    }
    m_glwidget->updateGL();
  }

  void ViewerGui::on_actionOctree_structure_toggled(bool enabled) {
    for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); 
         it != m_octrees.end(); ++it) {
      it->second.octree_drawer->enableOcTree(enabled);
    }
    m_glwidget->updateGL();
  }

  void ViewerGui::on_actionFree_toggled(bool enabled) {
    for (std::map<int, OcTreeRecord>::iterator it = m_octrees.begin(); 
         it != m_octrees.end(); ++it) {
      it->second.octree_drawer->enableFreespace(enabled);
    }
    m_glwidget->updateGL();
  
  }

  void ViewerGui::on_actionSelected_toggled(bool enabled) {
    // if(m_octreeDrawer) {
    //   m_octreeDrawer->enableSelection(enabled);

    //   // just for testing, you should set the selection somewhere else and only enable it here:
    //   if (enabled){
    //     std::list<OcTreeVolume> selection;
    //     std::pair<octomath::Vector3, double> volume(octomath::Vector3(0.0, 0.0, 0.0), 0.2);
    //     selection.push_back(volume);
    //     m_octreeDrawer->setOcTreeSelection(selection);

    //   } else{
    //     m_octreeDrawer->clearOcTreeSelection();
    //   }
    //   m_glwidget->updateGL();
    // }
  }


  void ViewerGui::on_action_bg_black_triggered() {
    m_glwidget->setBackgroundColor( QColor(0,0,0) );
    m_glwidget->qglClearColor( m_glwidget->backgroundColor() );
  }

  void ViewerGui::on_action_bg_white_triggered() {
    m_glwidget->setBackgroundColor( QColor(255,255,255) );
    m_glwidget->qglClearColor( m_glwidget->backgroundColor() );
  }

  void ViewerGui::on_action_bg_gray_triggered() {
    m_glwidget->setBackgroundColor( QColor(117,117,117) );
    m_glwidget->qglClearColor( m_glwidget->backgroundColor() );
  }

  void ViewerGui::on_savecampose_triggered() {
    QString filename = QFileDialog::getSaveFileName(this, "Save Viewer State", "camera.xml", "Camera/State file (*.xml)");
    if (!filename.isEmpty()) {
      saveCameraPosition(filename.toAscii().constData());
    }
  }

  void ViewerGui::on_loadcampose_triggered() {
    QString filename = QFileDialog::getOpenFileName(this, "Load Viewer State", "camera.xml", "Camera/State file (*.xml)");
    if (!filename.isEmpty()) {
      loadCameraPosition(filename.toAscii().constData());
    }
  }



  void ViewerGui::saveCameraPosition(const char* filename) const {
    // HACK get non-const pointer to myself
    ViewerWidget* aux = const_cast<ViewerWidget*>( m_glwidget);
    aux->setStateFileName(QString(filename));
    aux->saveStateToFile();
    aux->setStateFileName(QString::null);
  }

  void ViewerGui::loadCameraPosition(const char* filename) {
     m_glwidget->setStateFileName(QString(filename));
     m_glwidget->restoreStateFromFile();
     m_glwidget->setStateFileName(QString::null);
  } 


}


